﻿#ifndef IMU_H
#define IMU_H

#include <iostream>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "ekf_a.h"
#include <vector>
#include <fstream>
#include <QString>

class VarBuf{
public:
    VarBuf(int s = 10):size(s),g_(1)
    {
        vec.resize(size,g_);
        it = vec.begin();
        sum_ = g_*size;
        _var = 0;
        index = 0;
    }
    int get_size()
    {
        return size;
    }
    void resize(int s)
    {
        size = s;
        vec.resize(size,g_);
        sum_ = g_*size;
        it = vec.begin();
    }
    void add(double norm)
    {
//        sum_ = sum_ - *it + norm;
        sum_ = sum_+norm-vec[index];
        _mean = sum_ / size;
//        *it++ = norm;
        vec[index] = norm;
        index++;
//        if(it == vec.end()) { it = vec.begin(); }
        if(index >= size){
            index = 0;
        }

    }
    double var(){
        _var = 0;
        std::for_each (vec.begin(), vec.end(), [&](const double d) {
            _var  += (d-_mean)*(d-_mean);
        });
        _var /= (size-1);
        return _var;
    }
    double sum(){
        return sum_;
    }
    double mean(){
        return _mean;
    }

private:
    int size;
    int g_;
    int index = 0;
    std::vector<double> vec;
    std::vector<double>::iterator it;
    double _var;
    double sum_;
    double _mean;
};

class ZVD{
public:
    ZVD(int var_size = 20):acc_norm_size(var_size),_acc_up(1.004),_acc_bottom(0.996),_acc_var_limit(0.0001)
    {
        acc_norm_var_buf = new VarBuf(acc_norm_size);
        count = 0;
    }
    void set_norm_buf_size(int var_size);
    bool update(Eigen::Vector3d acc);
    void set_norm_threshold(double up, double bottom);
    void set_norm_var_limit(double up);
    void set_duration_in_out(int in, int out);

private:
    bool is_stationary = false;
//    bool is_stationary_lastdt = false;
    double _acc_up = 1.004;//10.04;
    double _acc_bottom =0.996;//9.96;
    double _acc_var_limit = 0.001;
    double acc_norm =10;
    int acc_norm_size = 20;
    int duration_in = 10;
    int duration_out = 3;
    int count = 0;
    VarBuf *acc_norm_var_buf;
};
struct AngleData
{
   double time;
   double angle;
};
class IMU {
public:
    IMU();
    ~IMU();

    enum STATE{
        MOVING = 0,
        STATION
    };

    void set_acc_norm_threshold(double up, double bottom);
    void set_acc_norm_var_limit(double up);

    Eigen::Vector3d & get_rotation_axis();
    QString calculate_rotation_axis();
    QString set_rotation_starting_point();
    STATE update_data( Eigen::Vector3d &);
    double get_angle();
    void importAsciiData ( const char *filename);
    inline void clear_acc_buf(){
        acc_buf.clear();
    }
    inline bool is_init_first_acc(){ return is_init_acc_; }
    inline void reset_first_acc(){ is_init_acc_ = false; }
    inline bool is_rotation_axis_calculated(){ return is_rotation_axis_get; }
    inline void reset_rotation_axis(){ acc_buf.clear(); init_acc_buf.clear(); is_rotation_axis_get = false; }

    inline Eigen::Vector3d & get_first_acc(){ return init_acc; }

//    inline void clear_angle_buf(){ angle_buf.clear(); }
    inline bool is_start_angle() { return start_angle; }

//    bool is_start_measure()

private:
    bool is_rotation_axis_get = false;
    bool is_start_measure = false;
    double angle;
    Eigen::Vector3d rotation_axis;
    Eigen::Vector3d acc;
    Eigen::Vector3d gyro;
    EKF_A *ekf;
    ZVD *zvd;
    std::vector<Eigen::Vector3d> acc_buf; // 用于计算旋转轴的加速度缓存，在reset_rotation_axis函数中清空
    Eigen::Vector3d init_acc;
    std::vector<Eigen::Vector3d> init_acc_buf; // 用于计算起点的加速度缓存，只要静止就记录，一动就清空, 重置旋转轴时也清空

    bool is_init_acc_ = false;
    bool start_angle = false;
};



#endif // IMU_H
